#include "line_pid.h"

// PID参数
float KP_x, KI_x, KD_x;
int error_x, error_last_x, integral_x;
uint8_t x1, x2, x3, x4, x5, x6, x7, x8;
int err;

// 传感器权重
int weights[8] = {-4, -3, -2, -1, 1, 2, 3, 4};

void init_x_PID(void) {
    KP_x = KPx;
    KI_x = KIx;
    KD_x = KDx;
    error_x = 0;
    error_last_x = 0;
    integral_x = 0;
}

int PID_count_x(void) {
    int pwmoutx = 0;
    LineWalking();

    error_x = err;

    // PID计算
    pwmoutx = KP_x * error_x 
              + KI_x * integral_x 
              + KD_x * (error_x - error_last_x);

    integral_x += error_x;
    error_last_x = error_x;

    // 死区处理
    if (abs(error_x) < 2) {
        error_x = 0;
        integral_x = 0;
    }

    // 积分限幅
    if (integral_x > 200) integral_x = 200;
    else if (integral_x < -200) integral_x = -200;

    // PWM输出限幅
    pwmoutx = constrain(pwmoutx, -255, 255);

    return pwmoutx;
}

void LineWalking(void) {
    static int last_valid_err = 0;
    static int lost_count = 0;
    static int err_filter[3] = {0};
    static int filter_index = 0;

    uint8_t sensors[8] = {x1, x2, x3, x4, x5, x6, x7, x8};
    int sum = 0;
    int count = 0;
    int current_err = 0;

    // 读取传感器状态
    for (int i = 0; i < 8; i++) {
        if (sensors[i] == 1) {
            sum += weights[i];
            count++;
        }
    }

    // 处理检测结果
    if (count >= 3) {
        current_err = sum;
        last_valid_err = current_err;
        lost_count = 0;
        err_filter[filter_index] = current_err;
        filter_index = (filter_index + 1) % 3;
    } else if (count > 0 && count < 3) {
        current_err = sum * 2;
        last_valid_err = current_err;
        lost_count = 1;
        err_filter[filter_index] = current_err;
        filter_index = (filter_index + 1) % 3;
    } else {
        lost_count++;
        if (lost_count <= 10) {
            current_err = last_valid_err;
        } 
        else if (lost_count <= 30) {
            int search_intensity = 6 + (lost_count - 10) / 2;
            current_err = (last_valid_err > 0) ? search_intensity : -search_intensity;
        } 
        else {
            int search_pattern = (lost_count / 10) % 2;
            current_err = (search_pattern == 0) ? 8 : -8;
            if (lost_count == 31) {
                uploadData("status", "LINE_LOST");
            }
        }
        err_filter[filter_index] = current_err;
        filter_index = (filter_index + 1) % 3;
    }

    // 滤波和限制
    err = (err_filter[0] + err_filter[1] + err_filter[2]) / 3;
    err = constrain(err, -8, 8);
}

void Car_line_track(void) {
    int PWMx;
    if (track_flag) {
        PWMx = PID_count_x();
        Set_speed(Speed_Line, PWMx);
        char buffer[20];
        sprintf(buffer, "err= %d\r\n", err);
        uploadData("track", buffer);  
    }
}
